/*
 * trans.c, transports for SmartCam skeleton system
 */

#define _XOPEN_SOURCE 500

#include <string.h>
#include <errno.h>
#include <log.h>
#include <timer.h>
#include <scu.h>
#include <signal.h>

#include "settings.h"
#include "scc.h"

log_default_streams_t sccl_trans;
extern scos_mutex_t scc_trans_mutex;
extern scos_cond_t  scc_trans_condition;
extern table_t scc_trans_watches;

table_t scc_trans_windows, scc_trans_procs;

extern table_t scc_procs;

void sccTransportInit(void)
{
  scos_thread_t thread;

  logAddGroup();
  sccl_trans = logAddDefaultStreams(NULL);
  logEdit(sccl_trans.info, LOG_GROUP_SET_LEVEL);

  lprintf(STREAM(sccl_trans.notice), "Initializing buffer transport system");

  abortOnNegative(STREAM(sccl_trans.err), tblcreate(&scc_trans_windows,sizeof(scc_window_t)));
  abortOnNegative(STREAM(sccl_trans.err), tblcreate(&scc_trans_watches,sizeof(scc_watch_t)));
#ifdef DES
  abortOnNegative(STREAM(sccl_trans.err), tbladdslice(&scc_trans_watches, 1));
#endif
  abortOnNegative(STREAM(sccl_trans.err), tblcreate(&scc_trans_procs,sizeof(scc_transproc_t)));

  scos_mutex_init(&scc_trans_mutex);
  scos_cond_init(&scc_trans_condition);

#ifdef DES
  scos_thread_create_dma(&thread, sccWatchBufferThread, NULL);
#else
  scos_thread_create(&thread, sccWatchBufferThread, NULL);
#endif
}

void sccTransportCreate(scu_buf_t *buf, int src, int dest, int destbuf)
{
  scu_bufptrid_t r;
  scc_trans_param_t *param;
  scos_thread_t thread;

  lprintf(STREAM(sccl_trans.notice), "Setting up transport to %d.%d", dest, destbuf);

  r = scuBufferAddReader(buf);

  param = malloc(sizeof(*param));
  param->src = src;
  param->dest = dest;
  param->destbuf = destbuf;
  param->buf = buf;
  param->ptr = r;

#ifdef DES
  {
    /* Simulate DMA for processors which have a transport time of 0 */
    scs_target_t *t;
    scs_target_operation_t *to;

    abortOnNULL(STREAM(sccl_trans.err), tblget(&scs_targets, param->src, &t));
    abortOnNULL(STREAM(sccl_trans.err), tblget(&t->operations, TRANSPORT, &to));

    if (to->time > 0)
      scos_thread_create(&thread, sccTransportThread, (void*) param);
    else
      scos_thread_create_dma(&thread, sccTransportThread, (void*) param);
  }
#else
  scos_thread_create(&thread, sccTransportThread, (void*) param);
#endif
}

void *sccTransportThread(void *arg)
{
  scc_trans_param_t *param = (scc_trans_param_t*) arg;
  char buf[2*SCOS_MTU];
  int bytes, total=0;
  tmr_t timer = tmrInit();
  scc_proc_t *sp, *dp;

  abortOnNULL(STREAM(sccl_trans.err), tblget(&scc_procs, param->src, &sp));
  abortOnNULL(STREAM(sccl_trans.err), tblget(&scc_procs, param->dest, &dp));

  sprintf(buf, "%s->%s.%d", sp->name, dp->name, param->destbuf);
  logRegisterThread(buf);
  
  lprintf(STREAM(sccl_trans.notice), "Started");

  /* Start transmitting */
  do
  {
    bytes = scuBufferRead(param->buf, param->ptr, buf, SCOS_MTU,
			  SCU_BUF_BLOCK_ANY);
    if (!total) tmrStart(&timer);

    if (bytes > SCOS_MTU)
    {
      lprintf(STREAM(sccl_trans.err), "Writing %d bytes", bytes);
      lprintf(STREAM(sccl_trans.err), "param %p", param);
      lprintf(STREAM(sccl_trans.err), "%d %d %d", param->src, param->dest, param->destbuf);
      
      raise(SIGSEGV);
    }

#ifdef DES
    if (settings.simulate)
    {
      void simTransportTransmitter(int, int, int, int);
      
      simTransportTransmitter(param->src, param->dest, param->destbuf, bytes);
    }    
#endif

    sccTransportSend(param->src, param->dest, param->destbuf, bytes, buf);
    total = total + bytes;

#ifdef DES
    if (settings.simulate)
    {
      /* For fair scheduling */
      scos_yield();
    }
#endif
  }
  while (!scuBufferIsDry(param->buf, param->ptr));

  tmrStop(&timer);

  lprintf(STREAM(sccl_trans.notice),
	  "Buffer dry; destroying transport (%d bytes sent, %5.2f Mbps)",
	  total, (double)total/tmrGetValueInUs(&timer)*8.0);

  scuBufferRemoveReference(param->buf);

  /* Destroy transport */
  if (bytes)
    sccTransportSend(param->src, param->dest, param->destbuf, 0, NULL);
  free(param);

  return NULL;
}

void *sccWatchBufferThread(void* dummy)
{
  int id, watches;
  scc_ack_t ack;
  scc_transproc_t *p;
  scc_watch_t *watch;

  logRegisterThread("watch");
  lprintf(STREAM(sccl_trans.notice), "Started");

  ack.header.type = SCC_MSGTYPE_BUFFERACK;
  ack.header.size = sizeof(scc_ack_t)-sizeof(scc_hdr_t);
  ack.acknowledged = 0;

  while (1)
  {
    scos_mutex_lock(&scc_trans_mutex);
    
    id = -1; watches = 0;
#ifdef DES
    while (tblget(&scc_trans_watches, id = tblgetslicenexti(&scc_trans_watches, 1, id), &watch))
#else
    while (tblget(&scc_trans_watches, id = tblgetnexti(&scc_trans_watches, id), &watch))
#endif
    {
      //lprintf(STREAM(sccl_trans.notice), "Checking watch for buffer %d", watch->buffer);
    
      abortOnNULL(STREAM(sccl_trans.err), tblget(&scc_trans_procs, watch->source, &p));
      ack.window = MIN(p->querycallback(watch->buffer), SCC_MAX_WINDOW);

      if (ack.window >= SCOS_MTU)
      {
	ack.header.source = watch->source;
	ack.header.destination = watch->destination;
	ack.buffer = watch->buffer;

	lprintf(STREAM(sccl_trans.debug),
		"Sending NULL acknowledge for %d with new window %d",
		ack.buffer, ack.window);

	sccSendMessage((scc_hdr_t*)&ack);

        //lprintf(STREAM(sccl_trans.notice), "Removing %d from watches", watch->buffer);
	abortOnNegative(STREAM(sccl_trans.err), id = tbldel(&scc_trans_watches, id, 1));
      }
#ifdef DES
      else
      {
        //lprintf(STREAM(sccl_trans.notice), "Removing %d from signalled watches", watch->buffer);
	abortOnNegative(STREAM(sccl_trans.err), tblswitchslice(&scc_trans_watches, 1, id, SCU_TABLE_NONMEMBER));
      }
#endif

      watches++;
    }

#ifndef DES   
    if (!watches)
#endif
      scos_cond_wait(&scc_trans_condition, &scc_trans_mutex);

    scos_mutex_unlock(&scc_trans_mutex);
#ifndef DES
    scos_yield();
#endif
  }
  
  return NULL;
}

void sccTransportRegister(int proc, scc_datacallback_t datacallback,
                          scc_querycallback_t querycallback)
{
  int id;
  scc_transproc_t *p;

  lprintf(STREAM(sccl_trans.notice), "Registering callbacks for processor %d", proc);

  abortOnNegative(STREAM(sccl_trans.err), id = tbladdnewi(&scc_trans_procs, &p, proc));

  p->id = id;
  p->datacallback = datacallback;
  p->querycallback = querycallback;
}

void sccTransportSend(int src, int dest, int destbuf, unsigned long int length, char *data)
{
  char buf[SCOS_MTU+sizeof(scc_datamsg_t)];
  scc_datamsg_t *msg = (scc_datamsg_t*) buf;
  scc_window_t *window;

  msg->header.source = src;
  msg->header.destination = dest;

  if (length)
    msg->header.type = SCC_MSGTYPE_BUFFERDATA;
  else
    msg->header.type = SCC_MSGTYPE_BUFFERDRY;

  msg->header.size = sizeof(scc_datamsg_t)-sizeof(scc_hdr_t)+length;
  
  msg->buffer = destbuf;

#ifdef DES
  memcpy(&msg->data, data, MIN(length, 4));
#else
  memcpy(&msg->data, data, length);
#endif

  scos_mutex_lock(&scc_trans_mutex);
  if (!tblget(&scc_trans_windows, msg->buffer, &window))
  {
    lprintf(STREAM(sccl_trans.info), "Initializing window for %d to %d", destbuf, SCOS_MTU);
    abortOnNegative(STREAM(sccl_trans.err),
		    tbladdnewi(&scc_trans_windows, &window, msg->buffer));
    
    /* Buffer size should be at least SCOS_MTU */
    window->window = SCOS_MTU;
    scos_mutex_init(&window->lock);
    scos_cond_init(&window->condition);
  }
  scos_mutex_unlock(&scc_trans_mutex);

  scos_mutex_lock(&window->lock);
  while (window->window < length)
  {
    lprintf(STREAM(sccl_trans.debug), "Waiting for buffer %d to become available",
	    destbuf);
    scos_cond_wait(&window->condition, &window->lock);
  }
  window->window -= length;
  window->unacked += length;
  scos_mutex_unlock(&window->lock);

  lprintf(STREAM(sccl_trans.crawl), "Writing %d bytes to %d; window now %d with %d unacked",
          length, destbuf, window->window, window->unacked);
 
  sccSendMessage((scc_hdr_t*)msg);

  if (msg->header.type == SCC_MSGTYPE_BUFFERDRY)
  {
    lprintf(STREAM(sccl_trans.info), "Buffer dry; deleting window for %d", destbuf);
    scos_mutex_lock(&scc_trans_mutex);
    abortOnNegative(STREAM(sccl_trans.err), tbldel(&scc_trans_windows, msg->buffer, 1));
    scos_mutex_unlock(&scc_trans_mutex);
  }
}

void sccTransportReceive(scc_hdr_t *hdr)
{
  scc_ack_t myack, *theirack = (scc_ack_t*)hdr;
  scc_datamsg_t *msg=(scc_datamsg_t*)hdr;
  scc_transproc_t *p;
  scc_watch_t *watch;
  scc_window_t *window;

  abortOnNULL(STREAM(sccl_trans.err), tblget(&scc_trans_procs, hdr->destination, &p));

  switch (hdr->type)
  {
    case SCC_MSGTYPE_BUFFERDATA:
      p->datacallback(hdr);
#ifndef PSOS
      myack.header.source = hdr->destination;
      myack.header.destination = hdr->source;
      myack.header.type = SCC_MSGTYPE_BUFFERACK;
      myack.header.size = sizeof(scc_ack_t)-sizeof(scc_hdr_t);
      myack.buffer = msg->buffer;
      myack.acknowledged = hdr->size-(sizeof(scc_datamsg_t)-sizeof(scc_hdr_t));
      myack.window = MIN(p->querycallback(msg->buffer), SCC_MAX_WINDOW);
      
      lprintf(STREAM(sccl_trans.crawl),
	      "Acknowledging %ld bytes for %d with new window %d",
	      myack.acknowledged, myack.buffer, myack.window);

      sccSendMessage((scc_hdr_t*)&myack);

      if (myack.window < SCOS_MTU && !tblget(&scc_trans_watches, msg->buffer, NULL))
      {
	/* Add buffer to the list of watched buffers */
	lprintf(STREAM(sccl_trans.debug), "Window too low; adding new watch for %d",
		msg->buffer);
	scos_mutex_lock(&scc_trans_mutex);
	abortOnNegative(STREAM(sccl_trans.err),
	                tbladdnewi(&scc_trans_watches, &watch, msg->buffer));
	watch->source = hdr->destination;
	watch->destination = hdr->source;
	watch->buffer = msg->buffer;
	scos_cond_signal(&scc_trans_condition);
	scos_mutex_unlock(&scc_trans_mutex);
      }
#endif
      break;
    case SCC_MSGTYPE_BUFFERDRY:
      scos_mutex_lock(&scc_trans_mutex);
      if (tblget(&scc_trans_watches, msg->buffer, &watch))
      {
	lprintf(STREAM(sccl_trans.debug), "Buffer dry; deleting watch for %d", msg->buffer);
	abortOnNegative(STREAM(sccl_trans.err), tbldel(&scc_trans_watches, msg->buffer, 1));
      }
      scos_mutex_unlock(&scc_trans_mutex);
      p->datacallback(hdr);
      break;
    case SCC_MSGTYPE_BUFFERACK:
      scos_mutex_lock(&scc_trans_mutex);
      if (tblget(&scc_trans_windows, theirack->buffer, &window))
      {
	scos_mutex_lock(&window->lock);
	window->unacked -= theirack->acknowledged;
	window->window = theirack->window - window->unacked;
	scos_cond_signal(&window->condition);
	scos_mutex_unlock(&window->lock);
	lprintf(STREAM(sccl_trans.crawl),
		"Got ACK for %ld bytes of %d with window %ld; window now %ld with %ld unacked", 
		theirack->acknowledged, theirack->buffer, theirack->window, window->window, window->unacked);
      }
      scos_mutex_unlock(&scc_trans_mutex);
      break;
  }  
}
